/*
 * sensor.c
 *
 *  Created on: Nov 20, 2024
 *      Author: Vinson
 */


#include "sensor.h"
#include "math.h"
#include "micros.h"
#include "stdio.h"
#include "string.h"

#ifdef TLE_5012B
#include "tle5012b.h"
float g_angleSpeed = 0;

#elif defined(TLE_5012B_DMA)
#include "tle5012b_dma.h"

#endif

typedef struct _sensor_info {
	float   velocity;				/* 速度 */
	float   angle_prev;				/* 上一次获取的sensor 机械角度*/
	long    angle_prev_ts;          /* 上一次获取机械角度时的时间戳 */
	float   vel_angle_prev;         /* 上一次获取速度时，记录的角度值 */
	long    vel_angle_prev_ts;      /* 上一次获取速度时，记录的角度值，对应时间戳 */
	float   min_elapsed_time;       /* 计算速度时忽略的时间 */
	int32_t full_rotations;     	/* 圈数跟踪 */
	int32_t vel_full_rotations; 	/* 上一次计算速度时候的圈数 */
} sensor_info_t;

static sensor_info_t sensor_ctx;

/**
 * @brief  ：角度归一化[0~2PI]
 * @name   ：_normalizeAngle
 * @param  ：angle, 角度值
 * @return ：返回值，返回角度。
 * @details：将角度转化为弧度。
 */
static double _normalizeAngle(double angle)
{
	/* 取余数 */
	double a = fmod(angle, 2 * PI);
	return a >= 0 ? a : (a + 2 * PI);
}


/* @brief  ：获取编码器的角度(弧度值)
 * @name   ：getSensorAngle
 * @param  ：无
 * @return ：返回获取的编码器弧度值[0~2PI]
 * @details：无
 */
 int getSensorAngle(double *angleValue)
{
	double value = 0.0f;
	errorTypes error;

	/* Step1: 获取角度[-PI,PI] */
	error = getAngleValue(&value);
	if (error != NO_ERROR) {
		printf("getAngleValue error(%d) \n", error);
		return error;
	}

	/* Step2：转换为弧度[-PI, PI] */
	//value = AngleToRad(value);
	//printf("getAngleValue %f \n", value);

	/* Step3：归一化角度[0, 2PI] */

	*angleValue = _normalizeAngle(value);

	return error;

}

 /* @brief  ：获取电机的机械角度(弧度值)
  * @name   ：getMechanicalAngle
  * @param  ：无
  * @return ：返回电机此时机械角度[0~2PI]
  * @details：无
  */
float getMechanicalAngle()
{
	return sensor_ctx.angle_prev;
}

/* @brief  ：获取当前的位置（弧度）,带圈
 * @name   ：getFullAngle
 * @param  ：无
 * @return ：返回电机当前位置，带圈
 * @details：获取当前位置。sensor_update 之后调用
 */
float getFullAngle()
{
	float angle_prev = sensor_ctx.angle_prev;
	int32_t full_rotations = sensor_ctx.full_rotations;
    return (float)full_rotations * TWO_PI + angle_prev;
}

int32_t getFullRotations() {
    return sensor_ctx.full_rotations;
}


/* @brief  ：计算电机的速度(rad/s)
 * @name   ：getVelocity()
 * @param  ：无
 * @return ：返回电机的速度
 * @details：无
 */
#if 1
float getVelocity()
{
	/* Step1：计算采样时间 */
    float Ts = (sensor_ctx.angle_prev_ts - sensor_ctx.vel_angle_prev_ts) * 1e-6f;
    if (Ts < 0.0f) {    // handle micros() overflow - we need to reset vel_angle_prev_ts
		printf("ts < 0 \n");
        sensor_ctx.vel_angle_prev = sensor_ctx.angle_prev;
        sensor_ctx.vel_full_rotations = sensor_ctx.full_rotations;
        sensor_ctx.vel_angle_prev_ts = sensor_ctx.angle_prev_ts;
        return sensor_ctx.velocity;
    }

	/* Step2：如果采样时间太短，不更新速度 */
    if (Ts < sensor_ctx.min_elapsed_time)  {
		printf("Ts sample too short %f \n", sensor_ctx.velocity);
		return sensor_ctx.velocity; // don't update velocity if deltaT is too small
	}

	/* Step3：计算速度 */
	float velocity = 0.0f;
	int32_t dfull_rotations = sensor_ctx.full_rotations - sensor_ctx.vel_full_rotations;
    velocity = 1.0 * ((float)dfull_rotations * TWO_PI + (sensor_ctx.angle_prev - sensor_ctx.vel_angle_prev)) / Ts;

	/* Step4：保存为下一次循环使用 */
	sensor_ctx.vel_angle_prev = sensor_ctx.angle_prev;
    sensor_ctx.vel_full_rotations = sensor_ctx.full_rotations;
    sensor_ctx.vel_angle_prev_ts = sensor_ctx.angle_prev_ts;
	sensor_ctx.velocity = velocity;

    return sensor_ctx.velocity;
}
#else
float getVelocity()
{
	errorTypes error;
	float angleSpeed = 0;
	error = getAngleSpeed(&angleSpeed);

	if (error != NO_ERROR) {
		printf("getAngleSpeed error \n");
		return 0.0f;
	}

	return angleSpeed;
}

#endif

extern gpio_t dbg_out2;
/* @brief  ：更新当前的电机位置，圈数和时间戳信息
 * @name   ：sensor_update
 * @param  ：无
 * @return ：无
 * @details：无
 */
void sensor_update()
{
    /* Step1: 更新当前角度值和时间戳 */
	double val = 0.0f;
	int ret = 0;
	//gpio_set_value(&dbg_out2, 0);
	ret = getSensorAngle(&val);
    if ( ret < 0 ) {
		printf("get sensor angle error: ret = %d \n", ret);
		return ;
	}
   // gpio_set_value(&dbg_out2, 1);

    sensor_ctx.angle_prev_ts = micros();

	/* Step2：跟踪记录圈数 */
#ifdef TLE_5012B
    errorTypes error;
    int16_t numRev = 0;
    error = getNumRevolutions(&numRev);
    if (error != NO_ERROR) {
    	printf("getNumRevolutionse error: error = %d \n", error);
    	return;
    }
    sensor_ctx.full_rotations = numRev;
#else
    /*DMA 方式 */
    float d_angle = val - sensor_ctx.angle_prev;
    // if overflow happened track it as full rotation
    if(fabs(d_angle) > (0.8f*_2PI) ) {
	sensor_ctx.full_rotations += ( d_angle > 0 ) ? -1 : 1;
    }

    encoder_isr_prepare();
#endif

	/* Step4：保存下一次的电机转动的位置(0~2PI) */
    sensor_ctx.angle_prev = val;

}

void sensor_init()
{
	float tmp = 0;
	/* 初始化sensor 内部变量以确保电机平滑运动 */

	memset(&sensor_ctx, 0, sizeof(sensor_ctx));

	/* Step1：获取编码器值 */
	getSensorAngle((double *)&tmp);
	delay_us(1);

	/* Step2：再次获取编码器值，并记录此时时间戳 */
	getSensorAngle((double *)&tmp);
	sensor_ctx.vel_angle_prev = tmp;
	sensor_ctx.vel_angle_prev_ts = micros();
	delay_us(1000);

	/* Step3：获取编码器值, 并记录此时时间戳 */
	getSensorAngle((double *)&tmp);
	delay_us(1);
	getSensorAngle((double *)&tmp);
	sensor_ctx.angle_prev = tmp;
	sensor_ctx.angle_prev_ts = micros();

	/* f = 170Mhz / (2x4095 + 1) = 20751, T = 48us */
	sensor_ctx.min_elapsed_time = 0.000048;

}
